from pickle import TRUE
from turtle import goto
from cv2 import waitKey
import numpy as np
import cv2

def image_detection(image):
    check_flag = False
    detected_agents = []
    detected_landmarks = []
    detected_check=[]  #待修改
    new_obs_n = []
    comm = []
    entity_pos=[[],[]]
    other_pos = []
    other_vel = []
    check_pos = []
    # image = env.render("rgb_array")  #获得图片    (width, height, 3) RGB顺序
    image_pro = np.squeeze(image, axis=0)

    image_can = cv2.Canny(image_pro, 100, 200, 5)
            #将其转化为灰度图片
    gray = cv2.cvtColor(image_pro, cv2.COLOR_RGB2GRAY)
            #应用霍夫圆进行圆检测 矩形检测
    circles = cv2.HoughCircles(image_can, cv2.HOUGH_GRADIENT, 1, 15,  param1=100,param2=8,minRadius=10,maxRadius=25)
    # contours, _2 = cv2.findContours(image_can, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
    #检测check
        # if not check_flag:
        # for i in range(len(circles)):
        #     cnt = circles[i]
        #     x, y, w, h = cv2.boundingRect(cnt)
        #     r, g, b = image_pro[int(y+h/2), int(x+w/2)]
        #     if (g == 153 and r==204):
        #         x = ( x+w/2)/400-1
        #         y = -((y+h/2)/400-1)
        #         detected_check.append(np.array([x,y]))
        #         check_flag == True
        #         break
        #     else: detected_check.append(np.array([-0.5,-0.5]))

            # 确保至少发现一个圆，检测agents landmarks
    if circles is not None:
                 # 进行取整操作
        circles = np.round(circles[0, :]).astype("int")
        i = 0
        for (x, y, r) in circles:
            r, g, b = image_pro[int(y), int(x)]
            x =(x/400) - 1
            y=-((y/400)-1)
            if (g==153 and r==204):                             #检测打卡点
                detected_check.append(np.array([x, y])) 
                circles = np.delete(circles, i, axis = 0)
                print(circles)
                break
            i+=1
        for (x, y, r) in circles:
            x =(x/400) - 1
            y=-((y/400)-1)
            if r/800 < 0.02:                                                #检测agents
                detected_agents.append([x, y])
                check_pos.append(np.array([x,y])-detected_check[0]) #1x2
            else:
                detected_landmarks.append([x, y])          #检测landmarks
        detected_landmarks = np.array(detected_landmarks)
        detected_agents = np.array(detected_agents)
        for i in range(len(detected_agents)):
            for j in range(len(detected_landmarks)):
                entity_pos[i].append(detected_agents[i] - detected_landmarks[j])
        print(len(detected_agents))
        # if len(detected_agents) < 2:
        #     filepath = '/home/zane/my_RL/MADDPG_torch/test/screenshot1.png'
        #     cv2.imwrite(filepath, image_pro, [int(cv2.IMWRITE_PNG_COMPRESSION),3])
        other_pos.append(detected_agents[0] - detected_agents[1])
        other_pos.append(detected_agents[1] - detected_agents[0])
        obs_0 = np.concatenate([detected_agents[0]]+ [other_pos[0]] + [check_pos[0]] + entity_pos[0])
        obs_1 = np.concatenate([detected_agents[1]]+ [other_pos[1]] + [check_pos[1]] + entity_pos[1])
        new_obs_n.append(obs_0)
        new_obs_n.append(obs_1)
        return new_obs_n